#include "includes.h"

extern uint16 amp[4];
extern int amp_history;
extern uint32 amp_time;
extern int cnt[2];
extern int trace[AMP_HISTORY];

extern track_status gramb_track;
extern control_struct servo,motor[2],anti[2];
extern led_status led[4];

extern FSM_EVENT gramb_event;

char cmd[2];
int format;
int cmd_receive;

void pit0_isr(void){
    /* Control routine */
    data_acquire();
    gramb_track.intrack=is_intrack(gramb_track.intrack);
    gramb_track.islost=is_lost(gramb_track.islost);
    gramb_track.instraight=is_instraight();
    update_trace();

    gramb_track.servo_turn=servo_control();
    motor_control();
    led_set();

    control_update();

    PIT_CLEAR_FLAG(0);
}

/* amp[0]: left(C2),
 * amp[1]: right(B1)
 * amp[2]: left_middle(E20)
 * amp[3]: right_middle(E22)
 *
 * cnt[0]: left(B16)
 * cnt[1]: right(C5)
 */
void pit1_isr(void){
    /* System routine */
    int i;
    /*
    if(format==0)
        vscope_data_feedback(amp, cnt);
    else
        printf("\n%d#%d|\n", amp[0], amp[1]);
    */

    for(i=0; i<4; ++i)
        printf("%d ", amp[i]);
    printf("|%d %d\n", cnt[0], cnt[1]);

    PIT_CLEAR_FLAG(1);
}

void pit_isr(void){
    if(PIT_TFLG(0)&PIT_TFLG_TIF_MASK)
        pit0_isr();
    if(PIT_TFLG(1)&PIT_TFLG_TIF_MASK)
        pit1_isr();
}

void uart0_isr(void){
    /* Bluetooth command communication routine */
    int i;

    for(i=0; i<2; ++i)
        cmd[i]=uart0_pendchar(UART0_BASE_PTR);

    if(cmd[0]=='#'){
        if(format)
            event_buffer_push(&gramb_event, GRAMB_EVENT_RESUME);
        else
            event_buffer_push(&gramb_event, GRAMB_EVENT_STOP);
        format=!format;
    }

    cmd_receive=1;
}

void dma1_isr(void){
    DMA_CLEAR_FLAG(1);
    amp_process();
    dma_adc_activate();

    /*
    for(i=0; i<4; ++i)
        printf("%hu ", amp[i]);
    printf("\n");
    */
}
